#include "../include/CameraFactory.h"

#include <boost/algorithm/string.hpp>

#include "../include/CataCamera.h"
#include "../include/EquidistantCamera.h"
#include "../include/PinholeCamera.h"
#include "../include/PinholeFullCamera.h"
#include "../include/ScaramuzzaCamera.h"

#include "ceres/ceres.h"

boost::shared_ptr<CameraFactory> CameraFactory::m_instance;

CameraFactory::CameraFactory() {}

boost::shared_ptr<CameraFactory> CameraFactory::instance(void)
{
  if (m_instance.get() == 0)
  {
    m_instance.reset(new CameraFactory);
  }

  return m_instance;
}

CameraPtr CameraFactory::generateCamera(Camera::ModelType modelType,
                                        const std::string &cameraName,
                                        cv::Size imageSize) const
{
  switch (modelType)
  {
  case Camera::KANNALA_BRANDT:
  {
    EquidistantCameraPtr camera(new EquidistantCamera);

    EquidistantCamera::Parameters params = camera->getParameters();
    params.cameraName() = cameraName;
    params.imageWidth() = imageSize.width;
    params.imageHeight() = imageSize.height;
    camera->setParameters(params);
    return camera;
  }
  case Camera::PINHOLE:
  {
    PinholeCameraPtr camera(new PinholeCamera);

    PinholeCamera::Parameters params = camera->getParameters();
    params.cameraName() = cameraName;
    params.imageWidth() = imageSize.width;
    params.imageHeight() = imageSize.height;
    camera->setParameters(params);
    return camera;
  }
  case Camera::PINHOLE_FULL:
  {
    PinholeFullCameraPtr camera(new PinholeFullCamera);

    PinholeFullCamera::Parameters params = camera->getParameters();
    params.cameraName() = cameraName;
    params.imageWidth() = imageSize.width;
    params.imageHeight() = imageSize.height;
    camera->setParameters(params);
    return camera;
  }
  case Camera::SCARAMUZZA:
  {
    OCAMCameraPtr camera(new OCAMCamera);

    OCAMCamera::Parameters params = camera->getParameters();
    params.cameraName() = cameraName;
    params.imageWidth() = imageSize.width;
    params.imageHeight() = imageSize.height;
    camera->setParameters(params);
    return camera;
  }
  case Camera::MEI:
  default:
  {
    CataCameraPtr camera(new CataCamera);

    CataCamera::Parameters params = camera->getParameters();
    params.cameraName() = cameraName;
    params.imageWidth() = imageSize.width;
    params.imageHeight() = imageSize.height;
    camera->setParameters(params);
    return camera;
  }
  }
}

CameraPtr CameraFactory::generateCameraFromYamlFile(
    const std::string &filename)
{
  cv::FileStorage fs(filename, cv::FileStorage::READ);

  if (!fs.isOpened())
  {
    return CameraPtr();
  }

  Camera::ModelType modelType = Camera::MEI;
  if (!fs["model_type"].isNone())
  {
    std::string sModelType;
    fs["model_type"] >> sModelType;

    if (boost::iequals(sModelType, "kannala_brandt"))
    {
      modelType = Camera::KANNALA_BRANDT;
    }
    else if (boost::iequals(sModelType, "mei"))
    {
      modelType = Camera::MEI;
    }
    else if (boost::iequals(sModelType, "scaramuzza"))
    {
      modelType = Camera::SCARAMUZZA;
    }
    else if (boost::iequals(sModelType, "pinhole"))
    {
      modelType = Camera::PINHOLE;
    }
    else if (boost::iequals(sModelType, "PINHOLE_FULL"))
    {
      modelType = Camera::PINHOLE_FULL;
    }
    else
    {
      std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
      return CameraPtr();
    }
  }

  switch (modelType)
  {
  case Camera::KANNALA_BRANDT:
  {
    EquidistantCameraPtr camera(new EquidistantCamera);

    EquidistantCamera::Parameters params = camera->getParameters();
    params.readFromYamlFile(filename);
    camera->setParameters(params);
    return camera;
  }
  case Camera::PINHOLE:
  {
    PinholeCameraPtr camera(new PinholeCamera);

    PinholeCamera::Parameters params = camera->getParameters();
    params.readFromYamlFile(filename);
    camera->setParameters(params);
    return camera;
  }
  case Camera::PINHOLE_FULL:
  {
    PinholeFullCameraPtr camera(new PinholeFullCamera);

    PinholeFullCamera::Parameters params = camera->getParameters();
    params.readFromYamlFile(filename);
    camera->setParameters(params);
    return camera;
  }
  case Camera::SCARAMUZZA:
  {
    OCAMCameraPtr camera(new OCAMCamera);

    OCAMCamera::Parameters params = camera->getParameters();
    params.readFromYamlFile(filename);
    camera->setParameters(params);
    return camera;
  }
  case Camera::MEI:
  default:
  {
    CataCameraPtr camera(new CataCamera);

    CataCamera::Parameters params = camera->getParameters();
    params.readFromYamlFile(filename);
    camera->setParameters(params);
    return camera;
  }
  }

  return CameraPtr();
}
